#pragma once
#include <common.hpp>
#include <Math/Vector3.hpp>
#include <Math/Matrix3x3.hpp>
#include <Math/IterExitCond.hpp>
#include "ProjectionMat.hpp"
#include "SceneData.hpp"

//implementation of 11.1.1

namespace zzz{
class FundamentalMat : public Matrix3x3d
{
public:
  FundamentalMat(const MatrixBase<3,3,double> &b):Matrix3x3d(b){}
  FundamentalMat(const FundamentalMat &b):Matrix3x3d(b){}
  FundamentalMat(){}
  bool Create8(const vector<Vector2d> &ls, const vector<Vector2d> &rs);
  int Create8RANSAC(const vector<Vector2d> &ls, const vector<Vector2d> &rs, const double outlier_threshold, IterExitCond<double> &cond);
  int Create8RANSAC(PairData<double> &data, const double outlier_threshold, IterExitCond<double> &cond);

  bool CalEpipoles(Vector3d &left, Vector3d &right);

  double Residue(const Vector2d &l, const Vector2d &r) const;
};

class EssentialMat : public FundamentalMat
{
public:
  EssentialMat(const MatrixBase<3,3,double> &b):FundamentalMat(b){}
  EssentialMat(const EssentialMat &b):FundamentalMat(b){}
  EssentialMat(){}

  bool GetProjection(ProjectionMatd &P, const Vector2d &l, const Vector2d &r) const;
};
}